function result = RRTstar(param, p_start, p_goal)

% RRT* 
% credit : Anytime Motion Planning using the RRT*, S. Karaman, et. al.
% calculates the path using RRT* algorithm 
% param : parameters for the problem 
%   1) threshold : stopping criteria (distance between goal and current
%   node)
%   2) maxNodes : maximum nodes for rrt tree 
%   3) neighborhood : distance limit used in finding neighbors
%   4) obstacle : must be rectangle-shaped #limitation
%   5) step_size : the maximum distance that a robot can move at a time
%   (must be equal to neighborhood size) #limitation
%   6) random_seed : to control the random number generation
% p_start : [x;y] coordinates
% p_goal : [x;y] coordinates
% variable naming : when it comes to describe node, if the name is with
% 'node', it means the coordinates of that node or it is just an index of
% rrt tree
% rrt struct : 1) p : coordinate, 2) iPrev : parent index, 3) cost :
% distance
% obstacle can only be detected at the end points but not along the line
% between the points
% for cost, Euclidean distance is considered.
% output : cost, rrt, time_taken 
% whether goal is reached or not, depends on the minimum distance between
% any node and goal 

field1 = 'p';      %position
field2 = 'iPrev';  %partent
field3 = 'cost';
field4 = 'goalReached';

rng(param.random_seed);
tic;
start();

    function start()      
        rrt(1) = struct(field1, p_start, field2, 0, field3, 0, field4, 0);  
        N = param.maxNodes; % iterations
        j = 1;

%         while endcondition>param.threshold %&& j<=N     
        while ~rrt(end).goalReached && j<=N
            sample_node = getSample(param.searchFeild);
%             plot(sample_node(1), sample_node(2), '.g');
%             text(sample_node(1), sample_node(2), strcat('random',num2str(j)))
            
%             idx = 1;
%             mincost = rrt(1).cost + norm(sample_node-rrt(1).p);
%             for k = 2:length(rrt)
%                 cost = rrt(k).cost + norm(sample_node-rrt(k).p);
%                 if cost < mincost
% %                     mincost = cost;
%                     idx = k;         %寻找总体代价最低的潜在父节点索引
%                 end
%             end
            
            nearest_node_ind = findNearest(rrt, sample_node);
%             plot(rrt(nearest_node_ind).p(1), rrt(nearest_node_ind).p(2), '.g');
%             text(rrt(nearest_node_ind).p(1), rrt(nearest_node_ind).p(2), strcat('nearest', num2str(j)));
            new_node = steering(rrt(nearest_node_ind).p, sample_node);
%             min_node = rrt(idx).p;    %连接sample_node后能使sample_node到根节点代价cost最小的rrt树中已经存在的节点（记为min_node）
            if (ObsFree(rrt(nearest_node_ind).p, new_node, param.resolution)==1)
%                 plot(new_node(1), new_node(2), '.g');
%                 text(new_node(1), new_node(2)+3, strcat('steered: new node', num2str(j)))
                neighbors_ind = getNeighbors(rrt, new_node);
                if(~isempty(neighbors_ind))
                    parent_node_ind = chooseParent(rrt, neighbors_ind, nearest_node_ind,new_node);
%                     plot(rrt(parent_node_ind).p(1), rrt(parent_node_ind).p(2), '.g');
%                     text(rrt(parent_node_ind).p(1), rrt(parent_node_ind).p(2)+3, strcat('parent', num2str(j)));
                else
                    parent_node_ind = nearest_node_ind;
                end
                rrt = insertNode(rrt, parent_node_ind, new_node);
%                 if (~isempty(neighbors_ind))
%                     rrt = reWire(rrt, neighbors_ind, parent_node_ind, length(rrt));
%                 end
                if norm(sample_node-p_goal) <= param.threshold
                    rrt = setReachGoal(rrt);
                end
            end
            j = j + 1;
        end
        setPath(rrt,p_goal);
%         text1 = strcat('Total number of generated nodes:', num2str(j-1))
%         text1 = strcat('Total number of nodes in tree:', length(rrt))
    end
    
    function rrt=setReachGoal(rrt)
        rrt(end).goalReached = 1;
    end
    
    
    function setPath(rrt,p_goal)
        for i = 1: length(rrt)
            p1 = rrt(i).p;
%             rob.x = p1(1); rob.y=p1(2);
            plot(p1(1),p1(2),'.b')
            child_ind = find([rrt.iPrev]==i);
            for j = 1: length(child_ind)
                p2 = rrt(child_ind(j)).p;
                plot([p1(1),p2(1)], [p1(2),p2(2)], 'b', 'LineWidth', 1);
            end
        end 
        
        [cost,i] = getFinalResult(rrt);
        result.cost = cost;
        result.rrt = [rrt.p];
        result.path = [p_goal,];  %先将目标点存在path变量中,虽然p_goal==rrt(i).p
        while i ~= 0
            p11 = rrt(i).p;
            plot(p11(1),p11(2),'m', 'Marker','.', 'MarkerSize', 15);
            i = rrt(i).iPrev;
            if i ~= 0
                p22 = rrt(i).p;                
%                 plot(p22(1),p22(2),'r', 'Marker', '.', 'MarkerSize', 15);
                plot([p11(1),p22(1)],[p11(2),p22(2)], '--m', 'LineWidth', 3);
                result.path = [result.path,p22];
            end 
        end  
        result.time_taken = toc;
        
    end

    function [value,min_node_ind] = getFinalResult(rrt)
        goal_ind = find([rrt.goalReached]==1);
        if ~(isempty(goal_ind))
            disp('Goal has been reached!');
            rrt_goal = rrt(goal_ind);
            value = min([rrt_goal.cost]);
            min_node_ind = find([rrt.cost]==value);
            if length(min_node_ind)>1
                min_node_ind = min_node_ind(1);
            end
        else
            disp('Goal has not been reached!');
            for i =1:length(rrt)
                norm_rrt(i) = norm(p_goal-rrt(i).p);
            end
            [~,min_node_ind]= min(norm_rrt); 
            value = rrt(min_node_ind).cost;
        end
    end

    function free = ObsFree(node1,node2,n) %n>=2,代表将线段分的段数
        free = 1;
        dx = (node2(1)-node1(1)) / n;
        dy = (node2(2)-node1(2)) / n;
        for i = 1:n
            if ~isObstacleFree([node1(1)+i*dx, node1(2)+i*dy])
                free = 0;
                break
            end
        end
    end

    % if it is obstacle-free, return 1.
    % otherwise, return 0
    function free=isObstacleFree(node_free)   %4
        free = 1;
        for i = 1: length(param.obstacles(:,1))
            obs = param.obstacles(i,:);
%             op1 = [obstacle(1), obstacle(2)];
%             op2 = [op1(1)+obstacle(3), op1(2)];
%             op3 = [op2(1), op1(2) + obstacle(4)];
%             op4 = [op1(1), op3(2)];
            nx = node_free(1);
            ny = node_free(2);
            ha = (nx-obs(1))^2 / obs(3)^2 + (ny-obs(2))^2 / obs(4)^2;
            if (ha <= 1)
                free = 0;
            end
        end 
    end
    
    function new_node=steering(nearest_node, random_node)   %3
       dist = norm(random_node-nearest_node);
       ratio_distance = param.step_size/dist;
       if ratio_distance < 1
           x = (1-ratio_distance).* nearest_node(1)+ratio_distance .* random_node(1);
           y = (1-ratio_distance).* nearest_node(2)+ratio_distance .* random_node(2);
           new_node = [x;y];
       else
           new_node = random_node;
       end
    end
    
    function rrt = reWire(rrt, neighbors, parent, new) %8
        for i=1:length(neighbors)
            cost = rrt(new).cost + norm(rrt(neighbors(i)).p - rrt(new).p);
            
            if (cost<rrt(neighbors(i)).cost)
%                 if norm(rrt(new).p-rrt(neighbors(i)).p)<param.step_size
% %                     plot(rrt(neighbors(i)).p(1), rrt(neighbors(i)).p(2), '.b');
%                     rrt(neighbors(i)).p = steering(rrt(new).p, rrt(neighbors(i)).p);
%                 end
%                 plot(rrt(neighbors(i)).p(1), rrt(neighbors(i)).p(2), '.m');
                rrt(neighbors(i)).iPrev = new;
                rrt(neighbors(i)).cost = cost;
            end
        end
    end
    

    function rrt = insertNode(rrt, parent, new_node)   %7
        rrt(end+1) = struct(field1, new_node, field2, parent, field3,...
            rrt(parent).cost + norm(rrt(parent).p-new_node), field4, 0);
    end
    
    function parent = chooseParent(rrt, neighbors, nearest, new_node)  %6
        min_cost = getCostFromRoot(rrt, nearest, new_node);
        parent = nearest;
        for i=1:length(neighbors)
            cost = getCostFromRoot(rrt, neighbors(i), new_node);
            if (cost<min_cost)
               min_cost = cost;
               parent = neighbors(i);
            end
        end
    end
    
    function cost = getCostFromRoot(rrt, parent, child_node)    %6.1
       cost =  rrt(parent).cost + norm(child_node - rrt(parent).p);
    end

    function neighbors = getNeighbors(rrt, node)    %5
        neighbors = [];
        for i = 1:length(rrt)
            dist = norm(rrt(i).p-node);
            if (dist<=param.neighbourhood)
               neighbors = [neighbors i];
            end
        end        
    end
    
    function node = getSample(sfeild)   %1
%         x = 0;
%         y = 0;
%         a = 0;
%         b = 200;
%         node = [x;y];
        ax = sfeild(1);  % ax是x的搜索范围的下界
        bx = sfeild(2);  % bx是x的搜索范围的上界
        ay = sfeild(3);  % ay是y的搜索范围的下界
        by = sfeild(4);  % by是y的搜索范围的上界
% %         node = [0, 0];
%         node(1) = (bx-ax) * rand(1) + ax;
%         node(2) = (by-ay) * rand(1) + ay;
        node = [0;0];
        free = 0;
        while ~free
            node(1) = (bx-ax) * rand(1) + ax;
            node(2) = (by-ay) * rand(1) + ay;
            if isObstacleFree(node)
                free = 1;
            end
        end
    end
    
    
    function indx = findNearest(rrt, n)   %2
        mindist = norm(rrt(1).p - n);
        indx = 1;
        for i = 2:length(rrt)
            dist = norm(rrt(i).p - n);
            if (dist<mindist)
               mindist = dist;
               indx = i;
            end
        end
    end 
    
end